#include <sstream>
#include <iostream>
#include <string>
#include <cstring>
#include <math.h>
#include <stdio.h>



#include <ros/ros.h>
#include <sensor_msgs/LaserScan.h>
#include <webots/Robot.hpp>

using namespace std;
using namespace webots;


class RWLaserServo
{
public:
  RWLaserServo();
  void init(webots::Robot*,ros::NodeHandle*,string);
  ~RWLaserServo();
  sensor_msgs::LaserScan getScan();
  void iterate();
private:
  double time_step, lms_width, lms_height;
  double v_res,v_max,v_min,m_pi,position;
  int n_iter,pt_sign;
  int v_count;
  Camera* LMS;
  const float *lms_values;
  Servo* PT_SERVO;
  ros::Publisher pub;
  ros::NodeHandle* n;
  sensor_msgs::LaserScan LidarScan;

};
